"""
@作者:sipeiliu@163.com

@节点身份:frame_image话题的订阅者,face_result服务的服务端

@节点任务:识别图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from cv_bridge import CvBridge # ros2与OpenCv图象转换类

# 其它库
import cv2 # OpenCv图像处理库


# 导入消息类型
from sensor_msgs.msg import Image # 图像消息类型
from facer_interfaces.srv import FaceResult # 自定义检测结果消息类型

"""识别图像节点类"""
class IdentifyImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('identify_image') # ros2节点父类初始化,节点名称为identify_image
    
        '''frame_image话题订阅者的相关代码'''
        self.subscription = self.create_subscription(Image, 'frame_image', self.listener_callback, 10) # 创建话题订阅者对象(消息类型、话题名、订阅者回调函数、队列长度)
        self.cv_bridge = CvBridge() # 创建图象转换对象，该对象可以将ros2的图像消息类型转换为OpenCv采集到的图像
        self.face_Classifier = cv2.CascadeClassifier('/home/sipeiliu/facer_ws/src/facer/haarcascade_frontalface_default.xml') # 加载人脸数据模型,创建人脸分类器对象，注意路径
        self.result = '没有检测到人脸' # 默认不存在人脸

        '''face_result服务服务端的相关代码'''
        self.srv = self.create_service(FaceResult, 'face_result', self.face_result_callback) # 创建服务服务端对象(消息类型、服务名、服务端回调函数)

    '''话题订阅者回调函数'''
    def listener_callback(self, ros2_image):       
        frame = self.cv_bridge.imgmsg_to_cv2(ros2_image, 'bgr8') # 将OpenCv采集到的图像转换为ros2的图像消息类型      
        faces = self.face_Classifier.detectMultiScale(frame) # 让人脸分类器判断人脸，返回一个元组列表，每个列表保存了人脸方框的四个坐标
        if len(faces) > 0: # 如果元组列表不为空
            self.result = '检测到人脸' # 只要有一帧图像出现了人脸，就判断结果为存在人脸

    '''服务服务端回调函数'''
    def face_result_callback(self, request, reponse):
        if request.apply_result == True: # 判断请求真假
            reponse.get_result = self.result # 传递检测结果
            return reponse # 回答服务的请求


def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

    node = IdentifyImage() # 创建ros2节点对象并进行初始化
    
    rclpy.spin(node) # 循环等待ros2退出
    
    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()